package org.test;

import org.apache.commons.math3.util.FastMath;

public class GPSUtils {

    public static final double EARTH_RADIUS = 6371.009; // 地球平均半径，单位为公里

    public static double haversine(double lon1, double lat1, double lon2, double lat2) {
        // 将十进制度数转化为弧度
        lon1 = Math.toRadians(lon1);
        lat1 = Math.toRadians(lat1);
        lon2 = Math.toRadians(lon2);
        lat2 = Math.toRadians(lat2);

        // haversine公式
        double dlon = lon2 - lon1;
        double dlat = lat2 - lat1;
        double a = FastMath.sin(dlat / 2) * FastMath.sin(dlat / 2) +
                FastMath.cos(lat1) * FastMath.cos(lat2) *
                        FastMath.sin(dlon / 2) * FastMath.sin(dlon / 2);
        double c = 2 * FastMath.asin(FastMath.sqrt(a));
        return c * EARTH_RADIUS * 1000; // 返回米
    }

    public static double distance(GPSPoint point1, GPSPoint point2) {
        return haversine(point1.lng, point1.lat, point2.lng, point2.lat);
    }

    public static GPSPoint gps2xy(GPSPoint point) {
        double R = GPSUtils.EARTH_RADIUS * 1000;
        // 经纬度转坐标
        double lng = point.lng * Math.PI / 180;
        double lat = point.lat * Math.PI / 180;
        double x = R * Math.cos(lat) * Math.cos(lng);
        double y = R * Math.cos(lat) * Math.sin(lng);
        return new GPSPoint(x, y);
    }

    public static GPSPoint xy2gps(GPSPoint point) {
        double R = GPSUtils.EARTH_RADIUS * 1000;
        double lng = Math.atan2(point.lat, point.lng) * 180 / Math.PI;
        double lat = Math.asin(point.lat / R) * 180 / Math.PI;
        return new GPSPoint(lng, lat);
    }
}
